#include "trajectory_predictor.hpp"
#include <iostream>
#include <algorithm>
#include <cmath>
#include <numeric>
using namespace std;

double gain_cal(double x) {
    if (x < 13.5) {
        return (1.0341957429588597 * log(0.7126536301164397 * x + 2.5813850030026853) + 0.02004352388383787);
    } else {
        return (1.3556224243118067 * log(2.5417186406523578 * log(x) - 6.127531547651636) + 3.2322785269552803);
    }
}

vector<double> vgain_cal(const vector<double>& x) {
    vector<double> result(x.size());
    transform(x.begin(), x.end(), result.begin(), gain_cal);
    return result;
}

//入参是显控比之前的真实报点鼠标位置
//返回是经过显控比换算之后，鼠标的位置
vector<PredictedPoint> TrajectoryPredictor::predict_trajectory(double x, double y, double t, KalmanFilter& kf, double& t_last, vector<PredictedPoint>& predictions) {
    // 更新状态转移矩阵
    kf.update_state_transition_matrix(t - t_last);
//    std::cout << "t-t_last: " << t - t_last << std::endl;

    // 预测步骤
    kf.predict();

    // 更新步骤：使用观测值更新卡尔曼滤波器
    Matrix<double> z(2, 1);
    z(0, 0) = x;
    z(1, 0) = y;
    kf.update(z);

    // 存储预测结果
//    std::vector<PredictedPoint> predictions;

    // 预测未来25个时间步的轨迹
    for (int i = 0; i < 25; ++i) {
        kf.update_state_transition_matrix(i);
        kf.extrapolate();
        Matrix<double> predicted_pos = kf.get_state_ex();
        //这里拿到真实报点的偏移量dx，dy
        PredictedPoint point {
                predicted_pos(0, 0),
                predicted_pos(1, 0),
                t + i
        };
        predictions.push_back(point);
    }
    // 更新上一次的时间
    t_last = t;
    predictions.pop_back();
    vector<PredictedPoint> dx_pred(predictions.size());
    for (int i = 1; i < predictions.size() + 1; ++i) {
        double dx = predictions[i].x - predictions[i -1].x;
        double dy = predictions[i].y - predictions[i -1].y;
        double dt = predictions[i -1].t;
        PredictedPoint point {
            dx, dy, dt
        };
        dx_pred[i-1] = point;
    }

    vector<double> vin(dx_pred.size());
    for (int i = 0; i < dx_pred.size(); ++i) {
        vin[i] = std::hypot(dx_pred[i].x, dx_pred[i].y);
    }

    vector<double> gain = vgain_cal(vin);
    vector<PredictedPoint> abs_dxdy_pred(dx_pred.size());
    for (size_t i = 0; i < dx_pred.size(); ++i) {
        double _x = dx_pred[i].x * gain[i];
        double _y = dx_pred[i].y * gain[i];
        double _t = dx_pred[i].t;
        PredictedPoint point {_x, _y, _t};
        abs_dxdy_pred[i] = point;
    }

    vector<PredictedPoint> abs_xy_pred = abs_dxdy_pred;
    partial_sum(abs_dxdy_pred.begin(), abs_dxdy_pred.end(), abs_xy_pred.begin(),
                [](PredictedPoint a, PredictedPoint b) {
                    return PredictedPoint(a.x + b.x, a.y + b.y, b.t);
    });
    vector<PredictedPoint> a;
    return abs_xy_pred;
}
